#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from inspire_hand.srv import set_pos
from std_msgs.msg import Int32MultiArray

class HandController:
    def __init__(self):
        rospy.init_node('hand_controller')
        rospy.loginfo("初始化灵巧手控制模块")

        self.set_position_client = rospy.ServiceProxy('inspire_hand/set_pos', set_pos)
        rospy.loginfo("等待灵巧手位置设置服务就绪")

        self.grasp_angle_subscriber = rospy.Subscriber('grasp_angle', Int32MultiArray, self.grasp_angle_callback)
        rospy.loginfo("订阅抓取角度话题")

    def grasp_angle_callback(self, msg):
        rospy.loginfo("接收到抓取角度消息: %s", msg.data)
        if len(msg.data) != 6:
            rospy.logwarn("接收到的抓取角度消息长度不正确")
            return
        # 在这里调用设置灵巧手位置的方法，根据接收到的抓取角度值来控制机械手
        self.set_hand_position(msg.data)

    def set_hand_position(self, grasp_angles):
        try:
            # 在这里根据抓取角度值来设置灵巧手的位置
            # 这里只是一个示例，具体的控制逻辑需要根据实际情况来实现
            # 可以调用灵巧手位置设置服务，将抓取角度值作为参数传递给服务
            response = self.set_position_client(*grasp_angles)
            if response.pos_accepted:
                rospy.loginfo("灵巧手位置设置成功!")
            else:
                rospy.logerr("灵巧手位置设置失败!")
        except rospy.ServiceException as e:
            rospy.logerr("调用服务失败: %s" % e)

if __name__ == '__main__':
    try:
        hand_controller = HandController()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
